{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98428077",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import gtsam"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ed4739a0",
   "metadata": {},
   "outputs": [],
   "source": [
    "interp_hertz = 200\n",
    "\n",
    "def read_optitrack_csv(filename, start_time_string):\n",
    "    dt =datetime.strptime(start_time_string, '%Y-%m-%d %I.%M.%S.%f%p')\n",
    "    timestamp = datetime.timestamp(dt)\n",
    "    df = pd.read_csv(filename, header=[5])\n",
    "    df = df.dropna()\n",
    "    df['Time (Seconds)'] += timestamp\n",
    "    df = df.rename(columns={\"Time (Seconds)\" : \"timestamp\", \"X.1\": \"tx\", \"Y.1\" : \"ty\", \"Z.1\" : \"tz\"})\n",
    "    return df\n",
    "\n",
    "optitrack = read_optitrack_csv(\"/data/05_14_2022/inside_loop_opti.csv\",\"2022-05-14 03.43.29.875PM\" )\n",
    "print(optitrack.head())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ed3577ab",
   "metadata": {},
   "outputs": [],
   "source": [
    "#interpolate missing readings in optitrack - Not needed now\n",
    "gt_data = optitrack.drop(['Frame', \"Unnamed: 9\"], axis=1)\n",
    "cols=['timestamp', 'tx', 'ty', 'tz', 'X', 'Y', 'Z', 'W']\n",
    "gt_data = gt_data[cols]\n",
    "print(gt_data.index)\n",
    "#gt_data = gt_data[gt_data.index % 20 == 0]\n",
    "theta = np.pi\n",
    "rot_apply = np.array([[np.cos(theta), np.sin(theta), 0],[-1*np.sin(theta), np.cos(theta), 0],[0,0,1]])\n",
    "print(rot_apply)\n",
    "for row in range(gt_data.shape[0]):\n",
    "    quat = np.array(gt_data.iloc[row, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    rot_1 =  rot_apply @ rot\n",
    "    #rint(gt_data.iloc[row])\n",
    "    gt_data.iloc[row, 4:8] = R.from_matrix(rot_1).as_quat()\n",
    "    gt_data.iloc[row, 1:4] = rot_apply @ gt_data.iloc[row, 1:4]\n",
    "    #rint(gt_data.iloc[row])\n",
    "    #print(rot)\n",
    "    #print(rot_1)\n",
    "    #print(\"---------\")\n",
    "#If we need to transform the data we can - Not needed for now\n",
    "gt_data.to_csv(\"/data/05_14_2022/inside_loop/results/inside_loop_opti.txt\", sep=' ', header=False, index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45f47429",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.spatial.transform import Rotation as R\n",
    "def undo_rot(quat):\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    rot_undone = rot @ (np.array([[0, 0, -1], [1, 0, 0], [0, -1, 0]]).T)\n",
    "    r = R.from_matrix(rot_undone)\n",
    "    return r.as_quat()\n",
    "    \n",
    "df = pd.read_csv(\"28_08_2020_trajectories/Tum_trajectrory.txt\", header=None, sep=' ')\n",
    "print(df.head())\n",
    "df2 = df.apply(lambda row: undo_rot(row.values[-4:]), axis=1)\n",
    "for i in range(df.shape[0]):\n",
    "    df.iloc[i, -4:] = df2[i] \n",
    "df.head()\n",
    "df.to_csv(\"28_08_2020_trajectories/Tum_trajectrory_conv.txt\",sep=' ', header=False, index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8ba0e25b",
   "metadata": {},
   "outputs": [],
   "source": [
    "trans_opti_first = np.array([[ 2.49348664284], [-2.04849290848], [1.07804751396]])  \n",
    "R_opti_first = R.from_quat([0.00842613261193, 0.0183087382466, -0.0918392241001, 0.995569944382]).as_matrix()\n",
    "T_opti_first = np.vstack((np.hstack((R_opti_first, trans_opti_first)) , np.array([[0,0,0,1]])))\n",
    "print(T_opti_first)\n",
    "df = pd.read_csv(\"28_08_2020_trajectories/Tum_trajectrory_conv.txt\", header=None, sep=' ')\n",
    "for i in range(df.shape[0]):\n",
    "    rot = R.from_quat(df.iloc[i, -4:]).as_matrix()\n",
    "    trans = np.array([df.iloc[i, 1: 4]]).T\n",
    "    T_first_rig = np.eye(4)\n",
    "    T_first_rig[:3, :3] = rot\n",
    "    T_first_rig[:3, 3:4]= trans\n",
    "    T_opti_rig = T_opti_first @ T_first_rig\n",
    "    df.iloc[i, -4:] = R.from_matrix(T_opti_rig[:3,:3]).as_quat()\n",
    "    df.iloc[i, 1:4] = T_opti_rig[:3, 3:].T\n",
    "df.to_csv(\"28_08_2020_trajectories/Tum_trajectrory_conv.txt\",sep=' ', header=False, index=False)\n",
    "#print(df.iloc[0])\n",
    "#rot = R.from_quat(df.iloc[0, -4:]).as_matrix()\n",
    "#trans = np.array([df.iloc[0, 1: 4]]).T\n",
    "#T_first_rig = np.eye(4)\n",
    "#T_first_rig[:3, :3] = rot\n",
    "#T_first_rig[:3, 3:4]= trans\n",
    "#T_opti_rig = T_opti_first @ T_first_rig\n",
    "#print(R.from_matrix(T_opti_rig[:3,:3]).as_quat())\n",
    "#print(T_opti_rig[:3, 3:].T)\n",
    "#df.iloc[0, -4:] = R.from_matrix(T_opti_rig[:3,:3]).as_quat()\n",
    "#df.iloc[0, 1:4] = T_opti_rig[:3, 3:].T\n",
    "#df.iloc[0, :3].shape\n",
    "#print(df.iloc[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7b21d188",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "### sidecam2 or right side cam\n",
    "T_ic5 = np.array([[-0.00927631, -0.0037559,  -0.99994992, -0.05856117],\n",
    "                 [-0.99995663,  0.00086417,  0.00927313,  0.052293  ],\n",
    "                 [ 0.00082929,  0.99999257, -0.00376376, -0.00098621],\n",
    "                 [ 0. ,         0. ,         0. ,         1.        ]]) \n",
    "\n",
    "#sidecam1 or leftside cam\n",
    "T_ic6 = np.array([[-0.0072078,  -0.00258906,  0.99997067,  0.06797633],\n",
    "                 [ 0.99996924,  0.00307407,  0.00721575, -0.01817503],\n",
    "                 [-0.00309266,  0.99999192,  0.00256682, -0.00964824],\n",
    "                 [ 0. ,         0. ,         0. ,         1.        ]])\n",
    "\n",
    "T_ic2 = np.array([[-0.99937297,  0.00280304,  0.03529594,  0.00024192],\n",
    "                 [ 0.03530387,  0.00283168,  0.99937261,  0.02224647],\n",
    "                 [ 0.00270134,  0.99999206, -0.00292886,  0.06674015],\n",
    "                 [ 0. ,         0. ,         0. ,         1.        ]])\n",
    "central, left, right\n",
    "\n",
    "T_c6_c2 = np.linalg.inv(T_ic6) @ T_ic2\n",
    "T_c5_c6 = np.linalg.inv(T_ic5) @ T_ic6\n",
    "\n",
    "print(T_c6_c2)\n",
    "print(T_c5_c6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bd2c4ee7",
   "metadata": {},
   "outputs": [],
   "source": [
    "df = pd.read_csv(\"ORB_stereo_noloop.txt\", header=None, sep=' ')\n",
    "df.head()\n",
    "df[0] = df[0]* 1e-9\n",
    "df.to_csv(\"ORB_stereo_noloop.txt\",sep=' ', header=False, index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3ee40735",
   "metadata": {},
   "outputs": [],
   "source": [
    "from scipy.spatial.transform import Rotation as R\n",
    "def undo_rot(quat):\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    rot_undone = rot @ (np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]))\n",
    "    r = R.from_matrix(rot_undone)\n",
    "    return r.as_quat()    \n",
    "df = pd.read_csv(\"/home/auv/ros_ws/src/light-fields-pack/scripts/python/optitrackGroundtruth.txt\", header=None, sep=' ')\n",
    "print(df.head())\n",
    "df2 = df.apply(lambda row: undo_rot(row.values[-4:]), axis=1)\n",
    "for i in range(df.shape[0]):\n",
    "    df.iloc[i, -4:] = df2[i]\n",
    "    #df.iloc[i, 1:4] = (np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]])) @ df.iloc[i, 1:4]\n",
    "df.head()\n",
    "df.to_csv(\"/home/auv/ros_ws/src/light-fields-pack/scripts/python/optitrackGroundtruth_1.txt\",sep=' ', header=False, index=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a43e1fc0",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "l = np.array([[-5.456451595755009,-0.08204201901518751, -34.18691026193634]])\n",
    "x_wc = np.array([[-0.3130189548504393, -0.02890853794180069, 0.9493068157020713, -4.446974418096292],\n",
    " [0.00211373344198866, 0.9995129720623712, 0.03113439914278101, -0.07702016144091782],\n",
    " [-0.9497445267203573, 0.01175223864262696, -0.3128054009310132, -34.52222460694547],\n",
    " [0, 0, 0, 1]])\n",
    "x_cw = np.linalg.inv(x_wc)\n",
    "l_c = x_cw @ np.vstack((l.T,[1]))\n",
    "print(l)\n",
    "print(l_c)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e969b6e1",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "##############################################################\n",
    "#PLOT TRACKING STATS\n",
    "##############################################################\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from typing import Iterable, Optional, Tuple\n",
    "from functools import reduce\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "\n",
    "# for creating a responsive plot\n",
    "%matplotlib widget\n",
    "\n",
    "def ellipsoid(rx: float, ry: float, rz: float,\n",
    "              n: int) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:\n",
    "    \"\"\"\n",
    "    Numpy equivalent of Matlab's ellipsoid function.\n",
    "\n",
    "    Args:\n",
    "        rx: Radius of ellipsoid in X-axis.\n",
    "        ry: Radius of ellipsoid in Y-axis.\n",
    "        rz: Radius of ellipsoid in Z-axis.\n",
    "        n: The granularity of the ellipsoid plotted.\n",
    "\n",
    "    Returns:\n",
    "        The points in the x, y and z axes to use for the surface plot.\n",
    "    \"\"\"\n",
    "    u = np.linspace(0, 2 * np.pi, n + 1)\n",
    "    v = np.linspace(0, np.pi, n + 1)\n",
    "    x = -rx * np.outer(np.cos(u), np.sin(v)).T\n",
    "    y = -ry * np.outer(np.sin(u), np.sin(v)).T\n",
    "    z = -rz * np.outer(np.ones_like(u), np.cos(v)).T\n",
    "\n",
    "    return x, y, z\n",
    "\n",
    "\n",
    "def plot_pose3_on_axes(axes, pose, color, axis_length=0.1, P=None, scale=1):\n",
    "    \"\"\"\n",
    "    Plot a 3D pose on given axis `axes` with given `axis_length`.\n",
    "\n",
    "    Args:\n",
    "        axes (matplotlib.axes.Axes): Matplotlib axes.\n",
    "        point (gtsam.Point3): The point to be plotted.\n",
    "        linespec (string): String representing formatting options for Matplotlib.\n",
    "        P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.\n",
    "    \"\"\"\n",
    "    # get rotation and translation (center)\n",
    "    gRp = pose[0:3, 0:3]  # rotation from pose to global\n",
    "    origin = pose[0:3,3]\n",
    "\n",
    "    # draw the camera axes\n",
    "    x_axis = origin + gRp[:, 0] * axis_length\n",
    "    line = np.append(origin[np.newaxis], x_axis[np.newaxis], axis=0)\n",
    "    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'r-')\n",
    "\n",
    "    y_axis = origin + gRp[:, 1] * axis_length\n",
    "    line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0)\n",
    "    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'g-')\n",
    "\n",
    "    z_axis = origin + gRp[:, 2] * axis_length\n",
    "    line = np.append(origin[np.newaxis], z_axis[np.newaxis], axis=0)\n",
    "    axes.plot(line[:, 0], line[:, 1], line[:, 2], 'b-')\n",
    "\n",
    "    # plot the covariance\n",
    "    if P is not None:\n",
    "        # covariance matrix in pose coordinate frame\n",
    "        # convert the covariance matrix to global coordinate frame\n",
    "        gPp = gRp @ P @ gRp.T\n",
    "        plot_covariance_ellipse_3d(axes, origin, gPp, color)\n",
    "\n",
    "def plot_covariance_ellipse_3d(axes,\n",
    "                               origin: np.ndarray,\n",
    "                               P: np.ndarray,\n",
    "                               color:np.ndarray,\n",
    "                               scale: float = 1,\n",
    "                               n: int = 8,\n",
    "                               alpha: float = 0.1) -> None:\n",
    "    \"\"\"\n",
    "    Plots a Gaussian as an uncertainty ellipse\n",
    "\n",
    "    Based on Maybeck Vol 1, page 366 \n",
    "    k=2.296 corresponds to 1 std, 68.26% of all probability\n",
    "    k=11.82 corresponds to 3 std, 99.74% of all probability\n",
    "\n",
    "    Args:\n",
    "        axes (matplotlib.axes.Axes): Matplotlib axes.\n",
    "        origin: The origin in the world frame.\n",
    "        P: The marginal covariance matrix of the 3D point\n",
    "            which will be represented as an ellipse.\n",
    "        scale: Scaling factor of the radii of the covariance ellipse.\n",
    "        n: Defines the granularity of the ellipse. Higher values indicate finer ellipses.\n",
    "        alpha: Transparency value for the plotted surface in the range [0, 1].\n",
    "    \"\"\"\n",
    "    k = 11.82\n",
    "    K= 1.0\n",
    "    U, S, _ = np.linalg.svd(P)\n",
    "\n",
    "    radii = k * np.sqrt(S)\n",
    "    radii = radii * scale\n",
    "    rx, ry, rz = radii\n",
    "\n",
    "    # generate data for \"unrotated\" ellipsoid\n",
    "    xc, yc, zc = ellipsoid(rx, ry, rz, n)\n",
    "\n",
    "    # rotate data with orientation matrix U and center c\n",
    "    data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \\\n",
    "        np.kron(U[:, 2:3], zc)\n",
    "    n = data.shape[1]\n",
    "    x = data[0:n, :] + origin[0]\n",
    "    y = data[n:2 * n, :] + origin[1]\n",
    "    z = data[2 * n:, :] + origin[2]\n",
    "\n",
    "    axes.plot_surface(x, y, z, alpha=alpha,color= color ) #cmap='hot'\n",
    "\n",
    "def parse_log(data_file):\n",
    "    df = pd.read_csv(data_file, header=[0],skipinitialspace=True) \n",
    "    df_poses = df[[\"pose_00\", \"pose_01\", 'pose_02', 'pose_03','pose_10', 'pose_11', 'pose_12',\n",
    "               'pose_13', 'pose_20', 'pose_21', 'pose_22', 'pose_23', 'pose_30', 'pose_31', 'pose_32', 'pose_33']]\n",
    "    df_cov = df[['cov_00', 'cov_01', 'cov_02', 'cov_10', 'cov_11', 'cov_12', 'cov_20', 'cov_21', 'cov_22']]\n",
    "\n",
    "    poses=[]\n",
    "    covs=[]\n",
    "    #extract poses\n",
    "    for row in range(df_poses.shape[0]):\n",
    "        p = np.array(df_poses.iloc[row, :]).reshape(4,4)\n",
    "        poses.append(p)\n",
    "\n",
    "    #extract covariances\n",
    "    for row in range(df_cov.shape[0]):\n",
    "        c = np.array(df_cov.iloc[row, :]).reshape(3,3)\n",
    "        covs.append(c)\n",
    "    FrameID = df['FrameID'].tolist()\n",
    "    stats_df = df[['FrameID','stamp', 'Num_matches_KF','Num_matches_KF_lms','Num_matches_KF_new',\n",
    "                   'Num_matches_localmap', 'Num_inliers_KF','Num_inliers_localmap','Num_triangulated',\n",
    "                   'inliers_view_0', 'inliers_view_1', 'inliers_view_2', 'triangulated_view_0',\n",
    "                   'triangulated_view_1', 'triangulated_view_2']]\n",
    "    Num_matches_KF = df[['FrameID', 'Num_matches_KF']]\n",
    "    Num_matches_KF_lms = df[['FrameID','Num_matches_KF_lms']]\n",
    "    Num_matches_KF_new = df[['FrameID','Num_matches_KF_new']]\n",
    "    Num_matches_localmap = df[['FrameID','Num_matches_localmap']]\n",
    "    Num_inliers_KF = df[['FrameID','Num_inliers_KF']]\n",
    "    Num_inliers_localmap = df[['FrameID','Num_inliers_localmap']]\n",
    "    Num_triangulated = df[['FrameID','Num_triangulated']]\n",
    "    return poses, covs, stats_df\n",
    "    #return poses, covs, FrameID, Num_matches_KF, Num_matches_KF_lms, Num_matches_KF_new, Num_matches_localmap, Num_inliers_KF, Num_inliers_localmap,Num_triangulated\n",
    "\n",
    "\n",
    "\n",
    "def plot_series(dfs, colName, names):\n",
    "    dfs_extracted =[]\n",
    "    for i in range(0, len(dfs)):\n",
    "        d = dfs[i][['FrameID', colName]]\n",
    "        d.rename(columns = {colName : colName+str(i)}, inplace = True)\n",
    "        dfs_extracted.append(d)\n",
    "    df_merged = reduce(lambda  left,right: pd.merge(left,right,on=['FrameID'],\n",
    "                                                    how='outer',sort=True), dfs_extracted)\n",
    "    df_merged = df_merged.iloc[2: , :]\n",
    "    df_final = df_merged.interpolate()\n",
    "    cmap = plt.cm.get_cmap('turbo', 10)\n",
    "    cmap=['r', 'g', 'b', 'c']\n",
    "    fig = plt.figure()\n",
    "    for i in range(len(dfs_extracted)):\n",
    "        plt.plot(df_final['FrameID'].tolist(), df_final[colName+str(i)].tolist(), c=cmap[i])\n",
    "    plt.legend(names)\n",
    "    plt.show()\n",
    "\n",
    "def plot_poses(poses_all, covs_all):\n",
    "    fig = plt.figure()\n",
    "    axes = fig.gca(projection='3d')\n",
    "    axis_labels=[\"X axis\", \"Y axis\", \"Z axis\"]\n",
    "    axes.set_xlabel(axis_labels[0])\n",
    "    axes.set_ylabel(axis_labels[1])\n",
    "    axes.set_zlabel(axis_labels[2])\n",
    "    cols=[np.array([1.0,0,0]),np.array([0,1.0,0])]\n",
    "    i=0\n",
    "    for i in range(len(poses_all)):\n",
    "        #color\n",
    "        poses = poses_all[i]\n",
    "        covs = covs_all[i]\n",
    "        col = cols[i]\n",
    "        i = i+1\n",
    "        print(\"color \", col)\n",
    "        for p,c in zip(poses, covs):\n",
    "            #print(p)\n",
    "            #print(c)\n",
    "            #print(\"-----------------\")\n",
    "            plot_pose3_on_axes(axes, p,np.array([0,1.0,0]), P=c)\n",
    "    \n",
    "    title=\"covariances\"\n",
    "    fig.suptitle(title)\n",
    "    fig.canvas.set_window_title(title.lower())\n",
    "\n",
    "poses_3, covs_3, stats_3 = parse_log(\"/home/auv/ros_ws/src/light-fields-pack/log/04_21_indoor/calib/poses_stats_isec_day_calib_5cams.txt\")\n",
    "poses_4, covs_4, stats_4 = parse_log(\"/home/auv/ros_ws/src/light-fields-pack/log/04_21_indoor/filter_intra/poses_stats_isec_day_filt_5cams.txt\")\n",
    "poses_5, covs_5, stats_5 = parse_log(\"/home/auv/ros_ws/src/light-fields-pack/log/04_25_2022_isec_day_fixed/poses_stats_isec_day_fixed_5cams.txt\")\n",
    "\n",
    "stats_3 = stats_3.iloc[4: , :]\n",
    "stats_4 = stats_4.iloc[4: , :]\n",
    "stats_5 = stats_5.iloc[4: , :]\n",
    "\n",
    "stats_list = [stats_3, stats_4, stats_5]\n",
    "plot_series(stats_list, 'Num_inliers_localmap', ['calib5', 'filtintra 5', 'fixed exp 5'])\n",
    "plot_series(stats_list, 'inliers_view_0', ['calib5', 'filtintra 5', 'fixed exp 5'])\n",
    "plot_series(stats_list, 'inliers_view_1', ['calib5', 'filtintra 5', 'fixed exp 5'])\n",
    "plot_series(stats_list, 'inliers_view_2', ['calib5', 'filtintra 5', 'fixed exp 5'])\n",
    "#plot_series(stats_list, 'inliers_view_3', ['calib5', '5cams', 'filtintra 5'])\n",
    "#plot_series(stats_list, 'inliers_view_4', ['calib5', '5cams', 'filtintra 5'])\n",
    "\n",
    "print(\"############ STATS###########\")\n",
    "print(\"Average Num of Inliers KF: {:0.2f}, {:0.2f}, {:0.2f}\".format(stats_3[\"Num_inliers_KF\"].mean(),\n",
    "                                                                         stats_4[\"Num_inliers_KF\"].mean(),\n",
    "                                                                         stats_5[\"Num_inliers_KF\"].mean()))\n",
    "print(\"Average Num of Inliers LocalMap: {:0.2f}, {:0.2f}, {:0.2f}\".format(stats_3[\"Num_inliers_localmap\"].mean(),\n",
    "                                                                         stats_4[\"Num_inliers_localmap\"].mean(),\n",
    "                                                                         stats_5[\"Num_inliers_localmap\"].mean()))\n",
    "print(\"Average Num of intra match inliers: {:0.2f}, {:0.2f}, {:0.2f}\".format((stats_3[\"Num_inliers_localmap\"] - stats_3[\"inliers_view_0\"]).mean(),\n",
    "                                                                      (stats_4[\"Num_inliers_localmap\"] - stats_4[\"inliers_view_0\"]).mean(),\n",
    "                                                                      (stats_5[\"Num_inliers_localmap\"] - stats_5[\"inliers_view_0\"]).mean()))\n",
    "print(\"Average Num of Mono Inliers : {:0.2f}, {:0.2f}, {:0.2f}\".format(stats_3[\"inliers_view_0\"].mean(),\n",
    "                                                                         stats_4[\"inliers_view_0\"].mean(),\n",
    "                                                                         stats_5[\"inliers_view_0\"].mean()))\n",
    "\n",
    "\n",
    "print(\"Average Num of Triangulated points: {:0.2f}, {:0.2f}, {:0.2f}\".format(stats_3[\"Num_triangulated\"].mean(),\n",
    "                                                                         stats_4[\"Num_triangulated\"].mean(),\n",
    "                                                                         stats_5[\"Num_triangulated\"].mean()))\n",
    "print(\"Number of keyframes: {:d}, {:d}, {:d}\".format(stats_3.shape[0],\n",
    "                                                          stats_4.shape[0],\n",
    "                                                          stats_5.shape[0]))\n",
    "\n",
    "#plot_poses([poses_2, poses_5], [covs_2, covs_5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5d32180d",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "# for creating a responsive plot\n",
    "%matplotlib widget\n",
    "\n",
    "data_file =\"../../log/02_23_2022/poses_landmark_5cams.txt\"\n",
    "def plt_hist_tracks(data_file, c):\n",
    "    with open(data_file, 'r') as f:\n",
    "        lines = f.readlines()\n",
    "    flag = True\n",
    "    lm_dict = {}\n",
    "    hist_data = []\n",
    "    kf_set = set()\n",
    "    lid= -1\n",
    "    for line in lines:\n",
    "        vals=line.split(\" \")\n",
    "        if vals[0] == 'e':\n",
    "            kf_set.add(vals[1])\n",
    "        if vals[0] == 'l':\n",
    "            if lid != -1:\n",
    "                lm_dict[lid] = list(kf_set)\n",
    "                hist_data.append(len(kf_set))\n",
    "            lid = vals[1]\n",
    "            kf_set.clear()\n",
    "    print(\"Num Landmarks : {:d} \".format(len(lm_dict)))\n",
    "    \n",
    "    bins= np.arange(2, 15)\n",
    "    n, x, _ = plt.hist(hist_data, bins=bins, facecolor=c, alpha=0.1, log=True)  # arguments are passed to np.histogram\n",
    "    bin_centers = 0.5*(x[1:]+x[:-1])\n",
    "    plt.plot(bin_centers,n, c=c)\n",
    "\n",
    "plt_hist_tracks(\"../../log/02_23_2022/poses_landmark_2cams.txt\",'r')\n",
    "plt_hist_tracks(\"../../log/02_23_2022/poses_landmark_3cams.txt\",'g')\n",
    "plt_hist_tracks(\"../../log/02_23_2022/poses_landmark_4cams.txt\",'b')\n",
    "plt_hist_tracks(\"../../log/02_23_2022/poses_landmark_5cams_3.txt\",'c')\n",
    "\n",
    "plt.title(\"Histogram of inlier track lengths\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e12a56c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "from scipy.signal import medfilt\n",
    "\n",
    "def cleanup_trajectory(data_file):\n",
    "    roll=[]\n",
    "    pit =[]\n",
    "    yaw =[]\n",
    "    x=[]\n",
    "    poses_5, covs_5, stats_5 = parse_log(data_file)\n",
    "    i=0\n",
    "    for p in poses_5:\n",
    "        rot = p[0:3, 0:3]\n",
    "        r = R.from_matrix(rot)\n",
    "        ya,pi,ro= r.as_euler('zyx', degrees=True)\n",
    "        if abs(ro) > 50.0:\n",
    "            if ro < 0:\n",
    "                ro = ro + 180.0\n",
    "            else:\n",
    "                ro = 180.0-ro\n",
    "        if abs(ya) > 50.0:\n",
    "            if ya < 0:\n",
    "                ya = ya + 180\n",
    "            else:\n",
    "                ya = 180.0-ya\n",
    "            \n",
    "        roll.append(ro)\n",
    "        pit.append(pi)\n",
    "        yaw.append(ya)\n",
    "        x.append(i)\n",
    "        i= i+1\n",
    "    #write the new trajectory\n",
    "    tstamps = stats_5['stamp'].tolist()\n",
    "    roll_fil = medfilt(roll, 5)\n",
    "    yaw_fil =  medfilt(yaw, 5)\n",
    "    traj = np.zeros((len(tstamps),8))\n",
    "    for i in range(len(tstamps)):\n",
    "        r = R.from_euler('zyx', [yaw[i], pit[i], roll[i]], degrees=True)\n",
    "        quat = r.as_quat()\n",
    "        traj[i, 0] = tstamps[i]\n",
    "        traj[i, 1:4] = poses_5[i][0:3, 3]\n",
    "        traj[i, 4:8] = quat\n",
    "    \n",
    "    df_traj = pd.DataFrame(traj)\n",
    "    df_traj.to_csv(\"../../log/02_23_2022/poses_stats_5cams_3_smooth.txt\", sep=\" \", index=False, header=None)\n",
    "    fig = plt.figure()\n",
    "    plt.plot(x,  medfilt(roll, 5), 'r-')\n",
    "    plt.plot(x, pit, 'g-')\n",
    "    plt.plot(x,  medfilt(yaw, 5), 'b')\n",
    "    plt.show()\n",
    "    \n",
    "data_file = \"../../log/02_23_2022/poses_stats_5cams_3.txt\"\n",
    "cleanup_trajectory(data_file)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "53741971",
   "metadata": {},
   "outputs": [],
   "source": [
    "######Process Kimera Log\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "\n",
    "data_file = \"/data/results/04_21_2022/traj_vio_noshift.csv\"\n",
    "df = pd.read_csv(data_file, header=[0],skipinitialspace=True) \n",
    "df = df[['#timestamp','x','y','z','qx','qy','qz','qw']]\n",
    "df['#timestamp'] = df['#timestamp']/1e+9\n",
    "\n",
    "Tci = np.array([[-0.00795927, 0.99994463, -0.00688388, -0.00460387],[0.00147386, 0.00689582, 0.99997514,\n",
    "                -0.01686081],[ 0.99996724, 0.00794893, -0.00152866, -0.03811018],[ 0., 0., 0., 1.]])\n",
    "\n",
    "for row in range(df.shape[0]):\n",
    "    quat = np.array(df.iloc[row, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    rot_1 = rot.dot(Tci[0:3,0:3].T)\n",
    "    df.iloc[row, -4:] = R.from_matrix(rot_1).as_quat()\n",
    "    print(row)\n",
    "    print(rot)\n",
    "    print(\"---------\")\n",
    "    #trans =  np.array(df.iloc[row, 1:4]).reshape(3,1)\n",
    "    #tmp = Tci[0:3,0:3] @ trans\n",
    "    #df.iloc[row, 1:4] = tmp.reshape(-1)\n",
    "df.to_csv(\"/data/results/04_21_2022/Tum_trajectory_kimera_no_shift.txt\", sep=\" \", index=False, header=None)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1a5485a2",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "dc361110",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "5e60a61f",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/tmp/ipykernel_135968/1545992659.py:30: DeprecationWarning: `np.float` is a deprecated alias for the builtin `float`. To silence this warning, use `float` by itself. Doing this will not modify any behavior and is safe. If you specifically wanted the numpy scalar type, use `np.float64` here.\n",
      "Deprecated in NumPy 1.20; for more details and guidance: https://numpy.org/devdocs/release/1.20.0-notes.html#deprecations\n",
      "  T_ci = np.zeros((4,4), np.float)\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "corners found\n",
      "corners found\n",
      "Image :  1650907610204368120\n",
      "index :  0\n",
      "GT Pose: \n",
      "[[ 8.98976328e-01 -5.61104485e-05  4.37997212e-01 -1.25970135e+00]\n",
      " [ 6.09542862e-02  9.90285097e-01 -1.24980008e-01  4.53064977e-01]\n",
      " [-4.33735099e-01  1.39051876e-01  8.90246280e-01 -3.67824729e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "expected Pose Traj: \n",
      "[[ 8.99011545e-01 -1.23734405e-04  4.37924910e-01 -1.25964493e+00]\n",
      " [ 6.09701496e-02  9.90296082e-01 -1.24885188e-01  4.53098291e-01]\n",
      " [-4.33659870e-01  1.38973573e-01  8.90295155e-01 -3.67811415e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "translation error:  0.00014839083591631927\n",
      "rotation error in degrees:  0.007534292136978357\n",
      "STamp: 1650907780.853183\n",
      "Corners not found\n",
      "STamp: 1650907781.253046\n",
      "Corners not found\n",
      "STamp: 1650907781.453423\n",
      "Corners not found\n",
      "STamp: 1650907781.652983\n",
      "Corners not found\n",
      "STamp: 1650907781.853002\n",
      "Corners not found\n",
      "STamp: 1650907782.053606\n",
      "Corners not found\n",
      "STamp: 1650907782.453424\n",
      "Corners not found\n",
      "STamp: 1650907783.253518\n",
      "Corners not found\n",
      "STamp: 1650907784.253587\n",
      "Corners not found\n",
      "STamp: 1650907785.053129\n",
      "Corners not found\n",
      "STamp: 1650907786.054472\n",
      "Corners not found\n",
      "STamp: 1650907787.454409\n",
      "Corners not found\n",
      "STamp: 1650907788.654716\n",
      "Corners not found\n",
      "STamp: 1650907789.904459\n",
      "Corners not found\n",
      "STamp: 1650907790.905274\n",
      "Corners not found\n",
      "STamp: 1650907792.304435\n",
      "Corners not found\n",
      "STamp: 1650907793.504295\n",
      "Corners not found\n",
      "STamp: 1650907794.503932\n",
      "Corners not found\n",
      "STamp: 1650907796.105191\n",
      "Corners not found\n",
      "STamp: 1650907796.704467\n",
      "Corners not found\n",
      "STamp: 1650907798.104569\n",
      "Corners not found\n",
      "STamp: 1650907799.504367\n",
      "Corners not found\n",
      "STamp: 1650907800.704542\n",
      "Corners not found\n",
      "STamp: 1650907801.50545\n",
      "Corners not found\n",
      "STamp: 1650907802.504219\n",
      "Corners not found\n",
      "STamp: 1650907803.304389\n",
      "Corners not found\n",
      "STamp: 1650907804.104652\n",
      "Corners not found\n",
      "STamp: 1650907804.904831\n",
      "Corners not found\n",
      "STamp: 1650907805.70464\n",
      "Corners not found\n",
      "STamp: 1650907806.50467\n",
      "Corners not found\n",
      "STamp: 1650907807.104974\n",
      "Corners not found\n",
      "STamp: 1650907807.903247\n",
      "Corners not found\n",
      "STamp: 1650907809.100241\n",
      "Corners not found\n",
      "STamp: 1650907810.299852\n",
      "Corners not found\n",
      "STamp: 1650907811.101265\n",
      "Corners not found\n",
      "STamp: 1650907811.902981\n",
      "Corners not found\n",
      "STamp: 1650907812.503628\n",
      "Corners not found\n",
      "STamp: 1650907813.503425\n",
      "Corners not found\n",
      "STamp: 1650907815.101503\n",
      "Corners not found\n",
      "STamp: 1650907816.100113\n",
      "Corners not found\n",
      "STamp: 1650907816.899729\n",
      "Corners not found\n",
      "STamp: 1650907817.698123\n",
      "Corners not found\n",
      "STamp: 1650907820.096716\n",
      "Corners not found\n",
      "STamp: 1650907821.897178\n",
      "Corners not found\n",
      "STamp: 1650907823.097119\n",
      "Corners not found\n",
      "STamp: 1650907823.896406\n",
      "Corners not found\n",
      "STamp: 1650907825.6965\n",
      "Corners not found\n",
      "STamp: 1650907827.096788\n",
      "Corners not found\n",
      "STamp: 1650907828.696832\n",
      "Corners not found\n",
      "STamp: 1650907829.697686\n",
      "Corners not found\n",
      "STamp: 1650907830.497325\n",
      "Corners not found\n",
      "STamp: 1650907831.296899\n",
      "Corners not found\n",
      "STamp: 1650907832.096402\n",
      "Corners not found\n",
      "STamp: 1650907832.896456\n",
      "Corners not found\n",
      "STamp: 1650907833.697169\n",
      "Corners not found\n",
      "STamp: 1650907834.497285\n",
      "Corners not found\n",
      "STamp: 1650907835.497329\n",
      "Corners not found\n",
      "STamp: 1650907837.297165\n",
      "Corners not found\n",
      "STamp: 1650907837.897292\n",
      "Corners not found\n",
      "STamp: 1650907838.297168\n",
      "Corners not found\n",
      "STamp: 1650907838.698296\n",
      "Corners not found\n",
      "STamp: 1650907839.297068\n",
      "Corners not found\n",
      "STamp: 1650907840.097123\n",
      "Corners not found\n",
      "STamp: 1650907840.89718\n",
      "Corners not found\n",
      "STamp: 1650907841.497106\n",
      "Corners not found\n",
      "STamp: 1650907842.097562\n",
      "Corners not found\n",
      "STamp: 1650907842.697386\n",
      "Corners not found\n",
      "STamp: 1650907843.297151\n",
      "Corners not found\n",
      "STamp: 1650907844.097229\n",
      "Corners not found\n",
      "STamp: 1650907845.297221\n",
      "Corners not found\n",
      "STamp: 1650907846.297095\n",
      "Corners not found\n",
      "STamp: 1650907847.296448\n",
      "Corners not found\n",
      "STamp: 1650907848.496466\n",
      "Corners not found\n",
      "STamp: 1650907849.296389\n",
      "Corners not found\n",
      "STamp: 1650907850.296373\n",
      "Corners not found\n",
      "STamp: 1650907850.898343\n",
      "Corners not found\n",
      "STamp: 1650907851.29953\n",
      "Corners not found\n",
      "STamp: 1650907851.9013\n",
      "Corners not found\n",
      "STamp: 1650907852.50103\n",
      "Corners not found\n",
      "STamp: 1650907852.90187\n",
      "Corners not found\n",
      "STamp: 1650907853.303009\n",
      "Corners not found\n",
      "STamp: 1650907854.103266\n",
      "Corners not found\n",
      "STamp: 1650907854.904127\n",
      "Corners not found\n",
      "STamp: 1650907855.503508\n",
      "Corners not found\n",
      "STamp: 1650907856.103226\n",
      "Corners not found\n",
      "STamp: 1650907856.703397\n",
      "Corners not found\n",
      "STamp: 1650907857.503123\n",
      "Corners not found\n",
      "STamp: 1650907858.10286\n",
      "Corners not found\n",
      "STamp: 1650907858.502776\n",
      "Corners not found\n",
      "STamp: 1650907858.902728\n",
      "Corners not found\n",
      "STamp: 1650907859.50255\n",
      "Corners not found\n",
      "STamp: 1650907860.302422\n",
      "Corners not found\n",
      "STamp: 1650907860.703285\n",
      "Corners not found\n",
      "STamp: 1650907861.103176\n",
      "Corners not found\n",
      "STamp: 1650907862.70321\n",
      "Corners not found\n",
      "STamp: 1650907863.703729\n",
      "Corners not found\n",
      "STamp: 1650907864.704378\n",
      "Corners not found\n",
      "STamp: 1650907865.504529\n",
      "Corners not found\n",
      "STamp: 1650907866.30459\n",
      "Corners not found\n",
      "STamp: 1650907867.304373\n",
      "Corners not found\n",
      "STamp: 1650907868.104253\n",
      "Corners not found\n",
      "STamp: 1650907869.904753\n",
      "Corners not found\n",
      "STamp: 1650907871.704463\n",
      "Corners not found\n",
      "STamp: 1650907873.105169\n",
      "Corners not found\n",
      "STamp: 1650907874.104414\n",
      "Corners not found\n",
      "STamp: 1650907875.904326\n",
      "Corners not found\n",
      "STamp: 1650907876.70448\n",
      "Corners not found\n",
      "STamp: 1650907877.304076\n",
      "Corners not found\n",
      "STamp: 1650907878.104775\n",
      "Corners not found\n",
      "STamp: 1650907878.504289\n",
      "Corners not found\n",
      "STamp: 1650907879.104162\n",
      "Corners not found\n",
      "STamp: 1650907879.904423\n",
      "Corners not found\n",
      "STamp: 1650907880.704768\n",
      "Corners not found\n",
      "STamp: 1650907881.505073\n",
      "Corners not found\n",
      "STamp: 1650907882.704284\n",
      "Corners not found\n",
      "STamp: 1650907883.704137\n",
      "Corners not found\n",
      "STamp: 1650907884.904014\n",
      "Corners not found\n",
      "STamp: 1650907885.954777\n",
      "Corners not found\n",
      "STamp: 1650907886.754504\n",
      "Corners not found\n",
      "STamp: 1650907887.753816\n",
      "Corners not found\n",
      "STamp: 1650907889.353829\n",
      "corners found\n",
      "Image :  1650907889353829167\n",
      "index :  331\n",
      "GT Pose: \n",
      "[[ 0.94361183 -0.0212321   0.33037238 -0.4369089 ]\n",
      " [ 0.07756803  0.98434122 -0.15828949  0.86059562]\n",
      " [-0.32183834  0.17499017  0.93048295 -5.23318656]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "expected Pose Traj: \n",
      "[[ 0.88352384 -0.01457279  0.46815943 -0.84707747]\n",
      " [ 0.08392594  0.98826531 -0.12762487  0.37420611]\n",
      " [-0.46080588  0.15205033  0.874379   -5.45660626]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "translation error:  0.6743362500806502\n",
      "rotation error in degrees:  8.756422414637191\n",
      "STamp: 1650907890.153525\n",
      "Corners not found\n",
      "STamp: 1650907890.954024\n",
      "Corners not found\n",
      "STamp: 1650907891.353637\n",
      "Corners not found\n",
      "STamp: 1650907892.153231\n",
      "corners found\n",
      "Image :  1650907892153231019\n",
      "index :  335\n",
      "GT Pose: \n",
      "[[ 0.97045008  0.00564621  0.2412359  -0.45048787]\n",
      " [ 0.02914317  0.98966567 -0.14040135  0.55476401]\n",
      " [-0.23953562  0.14328288  0.96025658 -4.34654515]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "expected Pose Traj: \n",
      "[[ 9.59741877e-01 -4.22899274e-03  2.80851643e-01 -2.00409137e-01]\n",
      " [ 4.55152640e-02  9.89013321e-01 -1.40644984e-01  2.15234945e-01]\n",
      " [-2.77171229e-01  1.47765917e-01  9.49389985e-01 -4.50839175e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "translation error:  0.4516787388206872\n",
      "rotation error in degrees:  2.43263743251568\n",
      "STamp: 1650907892.953577\n",
      "corners found\n",
      "Image :  1650907892953576438\n",
      "index :  336\n",
      "GT Pose: \n",
      "[[ 0.97723645  0.00865983  0.21197623 -0.3306576 ]\n",
      " [ 0.02136294  0.99007133 -0.138933    0.51047567]\n",
      " [-0.21107472  0.14029883  0.9673488  -4.05117602]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "expected Pose Traj: \n",
      "[[ 9.67684754e-01 -1.57077476e-03  2.52158182e-01 -6.59508518e-02]\n",
      " [ 3.76834023e-02  9.89652334e-01 -1.38449336e-01  1.66063922e-01]\n",
      " [-2.49331461e-01  1.43477490e-01  9.57730668e-01 -4.20762430e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "translation error:  0.4616981467513179\n",
      "rotation error in degrees:  2.4456640521076465\n",
      "STamp: 1650907893.753628\n",
      "corners found\n",
      "Image :  1650907893753628338\n",
      "index :  337\n",
      "GT Pose: \n",
      "[[ 0.98614287  0.00867947  0.16567106 -0.21480598]\n",
      " [ 0.01413264  0.99060516 -0.1360209   0.46369497]\n",
      " [-0.1652952   0.13647741  0.97675555 -3.75942391]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "expected Pose Traj: \n",
      "[[ 9.78564615e-01 -1.82903789e-03  2.05931903e-01  6.77300342e-02]\n",
      " [ 3.02184451e-02  9.90412215e-01 -1.34797956e-01  1.14947813e-01]\n",
      " [-2.03710922e-01  1.38131452e-01  9.69237619e-01 -3.90537748e+00]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "translation error:  0.4719678224339361\n",
      "rotation error in degrees:  2.425662100176064\n",
      "STamp: 1650907894.553652\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "corners found\n",
      "Image :  1650907894553652056\n",
      "index :  338\n",
      "GT Pose: \n",
      "[[ 0.98603978  0.00693975  0.16636522 -0.13013057]\n",
      " [ 0.01656468  0.99008653 -0.1394786   0.41624636]\n",
      " [-0.16568391  0.14028724  0.97614975 -3.44986278]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "expected Pose Traj: \n",
      "[[ 0.97865555 -0.00362585  0.20547548  0.16837299]\n",
      " [ 0.03284991  0.98974808 -0.13899503  0.06983898]\n",
      " [-0.20286498  0.14277811  0.96874156 -3.60234188]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "translation error:  0.4820293861432546\n",
      "rotation error in degrees:  2.364216256555294\n",
      "Average translation error :  0.5083420688459692\n",
      "Average rotational error :  3.6849204511983755\n"
     ]
    }
   ],
   "source": [
    "###################################################################\n",
    "#####  PROCESS the drift error from checkerboard              ##### \n",
    "###################################################################\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import cv2\n",
    "\n",
    "def draw(img, corners, imgpts):\n",
    "    corners = corners.astype(np.int32)\n",
    "    imgpts = imgpts.astype(np.int32)\n",
    "    corner = tuple(corners[0].ravel())\n",
    "    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (0,0,255), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (255,0,0), 5)\n",
    "    return img\n",
    "\n",
    "\n",
    "def getPoseFromChessBoard(img_inp, k, dist, squarSize):\n",
    "    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "    objp = np.zeros((16*9,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:16,0:9].T.reshape(-1,2)\n",
    "    objp = objp * squarSize\n",
    "    axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)\n",
    "    axis = axis * squarSize\n",
    "    T_ci = np.zeros((4,4), np.float)\n",
    "    ## detect chesssboard\n",
    "    ret, corners = cv2.findChessboardCorners(img_inp, (16,9),None, cv2.CALIB_CB_FAST_CHECK)\n",
    "   \n",
    "    if ret == True:\n",
    "        print(\"corners found\")\n",
    "        corners2 = cv2.cornerSubPix(img_inp,corners,(11,11),(-1,-1),criteria)\n",
    "        # Find the rotation and translation vectors.\n",
    "        _,rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, k, dist)\n",
    "        rot_mat, _ = cv2.Rodrigues(rvecs)\n",
    "        ## get pose WRT chessboard T_ci\n",
    "        T_ic = np.vstack((np.hstack((rot_mat, tvecs)), np.array([0, 0, 0, 1])))\n",
    "        T_ci = np.linalg.inv(T_ic)\n",
    "        # project 3D points to image plane\n",
    "        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, k, dist)\n",
    "        cv2.drawChessboardCorners(img_inp, (16,9), corners2, ret)\n",
    "        img = cv2.cvtColor(img_inp,cv2.COLOR_GRAY2RGB)\n",
    "        img = draw(img,corners2,imgpts)\n",
    "        cv2.imshow('img',img)\n",
    "        cv2.waitKey(0) & 0xff\n",
    "    else:\n",
    "        print(\"Corners not found\")\n",
    "    return T_ci\n",
    "    \n",
    "\n",
    "def computeError(df_traj, tStamp, k, dist, squareSize, patternSize, T_c0=np.eye(4)):\n",
    "    img_gray = cv2.imread(os.path.join(data_path,str(tStamp)+\".jpg\"), 0)\n",
    "    #clahe = cv2.createCLAHE(clipLimit = 5)\n",
    "    #img_gray = clahe.apply(img_gray)\n",
    "    T_ci_gt = getPoseFromChessBoard(img_gray, k, dist, squareSize)\n",
    "    if np.linalg.norm(T_ci_gt) == 0:\n",
    "        #print(\"Checkerboard not found\")\n",
    "        return -1, -1\n",
    "    t =round(tStamp*1e-9, 6)\n",
    "    print(\"Image : \", tStamp) \n",
    "    ind_df = df_traj[0].sub(t).abs().idxmin()\n",
    "    \n",
    "    #ind_df = (df_traj[0].tolist().index(t))\n",
    "    print(\"index : \", ind_df)\n",
    "    quat = np.array(df_traj.iloc[ind_df, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    trans = np.array(df_traj.iloc[ind_df,1:4]).reshape(3,1)\n",
    "    T_0i = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "    ## In Tum trajectory we have T_0i, pose WRT start, \n",
    "    ## GT = T_ci, expected T_ci^ = T_c0 * T_0i=\n",
    "    print(\"GT Pose: \")\n",
    "    print(T_ci_gt)\n",
    "    if T_ci_gt[3,3] > 2.0 :\n",
    "        print(\"calibrationj board is too far. estimate wont be accurate\")\n",
    "        return -1, -1\n",
    "    print(\"expected Pose Traj: \")\n",
    "    T_ci_exp = T_c0 @ T_0i\n",
    "    print(T_ci_exp)\n",
    "    #find delta pose\n",
    "    deltapose = np.linalg.inv(T_ci_exp) @ T_ci_gt\n",
    "    rotvec_cv,_ = cv2.Rodrigues(deltapose[0:3,0:3])\n",
    "    trans_error = np.linalg.norm(T_ci_exp[0:3, 3] - T_ci_gt[0:3, 3])\n",
    "    rot_error = np.rad2deg(np.linalg.norm(rotvec_cv))\n",
    "    print(\"translation error: \",trans_error, )\n",
    "    print(\"rotation error in degrees: \", rot_error)\n",
    "    return trans_error, rot_error\n",
    "\n",
    "\n",
    "\n",
    "# data_path=\"/data/04_14_2022_isec_lab_ground/isec_ground_night_2/cam0\"\n",
    "# traj_path = \"/data/results/04_14_2022/KeyFrameTrajectory_04_14_2022_stereo.txt\"\n",
    "# k = np.array([[886.6272107893, 0, 361.488867356257],\n",
    "#               [0, 886.7929657865, 284.068219958725],\n",
    "#               [0, 0, 1]])\n",
    "# dist = np.array([-0.21286163901817143, 0.17023197326272427, -9.485363152188766e-06,\n",
    "#     -0.0001386285980226874])\n",
    "#five_cams = [1649979950837768693,1649979951838062494, 1649979953036801584,1649979954237361874,1649979955237576739 ]\n",
    "#four_cams=[1649979951437556527, 1649979952637979681,1649979953837583386, 1649979954837175857,1649979955837720923  ]\n",
    "#three_cams = [1649979951838062494,1649979953036801584, 1649979954237361874,1649979955237576739 ]\n",
    "#two_cams = [1649979951437556527,1649979952436942290, 1649979953637951101, 1649979954636732071, 1649979955637119932]\n",
    "#orb_slam = [1649979955186799326,1649979955787700488, 1649979955987961614,1649979957087622430 ]\n",
    "#five_cams1 = [1649979954037088217, 1649979955038292781, 1649979956037060130 ]\n",
    "#\n",
    "# first_stamp = 1649979716237968880\n",
    "\n",
    "\n",
    "#data_path=\"/data/04_21_2022_isec_curry/bag_outdoor_nu/cam0\"/data/results/04_14_2022/KeyFrameTrajectory.txt\n",
    "#traj_path = \"/home/auv/ros_ws/devel/lib/LFApps/04_21_2022/outdoor/Curry_center_Orbslam3_stereo.txt\"\n",
    "#k = np.array([[887.6164997890, 0, 361.2404351028],\n",
    "#              [0, 888.007246603, 283.140828388],\n",
    "#              [0, 0, 1]])\n",
    "#dist = np.array([-0.21046072327790924, 0.15577979446322998, -0.0001708844706763513, -0.00022739337206347906])\n",
    "\n",
    "#five_cams = [1650577604747234911,1650577605647659897, 1650577609847147436 ,1650577610747154366 , 1650577611947129604, 1650577613147546904]\n",
    "#four_cams = [1650577604747234911, 1650577605647659897, 1650577609546509253, 1650577610447049664, 1650577611346696357, 1650577612547425164, 1650577613747880382]\n",
    "#three_cams = [1650577604747234911, 1650577605647659897, 1650577609546509253, 1650577610447049664, 1650577611346696357, 1650577612547425164, 1650577613747880382]\n",
    "#two_cams = [1650577604747234911,1650577605647659897, 1650577609847147436 ,1650577610747154366 , 1650577611947129604, 1650577613147546904]\n",
    "#orb_slam = [ 1650577604047963739,1650577605047238159, 1650577611246613269 ]\n",
    "\n",
    "#first_stamp = 1650576194947202033\n",
    "\n",
    "\n",
    "#data_path=\"/data/04_21_2022_isec_curry/bag_indoor_isec/cam0\"\n",
    "#traj_path = \"/data/results/04_21_2022/indoor/KeyFrameTrajectory.txt\"\n",
    "#k = np.array([[887.616499789026, 0, 361.2404351028],\n",
    "#              [0, 888.007246603, 283.140828388],\n",
    "#              [0, 0, 1]])\n",
    "#dist = np.array([-0.21046072327790924, 0.15577979446322998, -0.0001708844706763513, -0.00022739337206347906])\n",
    "\n",
    "#five_cams = [1650574607156144434]\n",
    "#four_cams = [1650574607156144434]\n",
    "#three_cams = [1650574606956391956]\n",
    "#two_cams = [1650577604747234911,1650577605647659897, 1650577609847147436 ,1650577610747154366 , 1650577611947129604, 1650577613147546904]\n",
    "#orb_slam = [ 1650577604047963739,1650577605047238159, 1650577611246613269 ]\n",
    "\n",
    "#first_stamp = 1650574403456143497 #1650574407056667763 #\n",
    "\n",
    "\n",
    "#data_path=\"/data/04_25_2022_isec_ground_dynamic/bag1/cam0\"\n",
    "#traj_path = \"/home/auv/ros_ws/devel/lib/LFApps/04_25_2022_isec_day_fixed/04_25_2022_isec_day_fixed_Orbslam3.txt\"\n",
    "#k = np.array([[886.728628930516, 0, 364.89350117142],\n",
    "#              [0, 887.300026814151, 286.96803692817],\n",
    "#              [0, 0, 1]])\n",
    "#dist = np.array([-0.21627336164628835, 0.17205009106186261, 1.633696614419175e-05, 0.0008008605016318086])\n",
    "#first_stamp = 1650907022320894118 #1650907004920428538\n",
    "\n",
    "#\n",
    "data_path=\"/data/04_25_2022_isec_ground_dynamic/isec_ground_day_2loops/cam0\"\n",
    "traj_path = \"/data/results/04_25_2022_4cams_traj.txt\"\n",
    "k = np.array([[886.7286289305, 0, 364.893501171],\n",
    "             [0, 887.3000268141, 286.968036928],\n",
    "            [0, 0, 1]])\n",
    "dist = np.array([-0.21627336164628835, 0.17205009106186261, 1.633696614419175e-05, 0.000800860501631808])\n",
    "first_stamp = 1650907610204368120\n",
    "#last_stamp =  1650907894553652056        #1650907892153231019\n",
    "\n",
    "## Get the time stamps and read images and check which ones have the checkerboard detected.\n",
    "## Or get the timestamps manually and read the images\n",
    "\n",
    "## read the tum format trajectory file\n",
    "df = pd.read_csv(traj_path, header=None,skipinitialspace=True, sep=' ')\n",
    "img_first = cv2.imread(os.path.join(data_path,str(first_stamp)+\".jpg\"), 0)\n",
    "#clahe = cv2.createCLAHE(clipLimit = 5)\n",
    "#img_first = clahe.apply(img_first)\n",
    "T_cs = getPoseFromChessBoard(img_first, k, dist, 0.081)\n",
    "\n",
    "# if start stamp is not the first pose\n",
    "#t =round(first_stamp*1e-9, 6)\n",
    "#ind_df = df[0].sub(t).abs().idxmin()\n",
    "#print(\"index170 : \", ind_df)\n",
    "#quat = np.array(df.iloc[ind_df, 4:8])\n",
    "#rot = R.from_quat(quat).as_matrix()\n",
    "#trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "#T_0s = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "#T_cs = T_cs @ np.linalg.inv(T_0s)  \n",
    "\n",
    "computeError(df, first_stamp, k, dist, 0.081, (9,9), T_cs)\n",
    "imgnames = os.listdir(data_path)\n",
    "imgnames.sort()\n",
    "imgstamps = np.array([ float(img[:-4])*1e-9 for img in imgnames])\n",
    "\n",
    "\n",
    "#check_stamp = 1650574602.756657\n",
    "#print(\"Chec stamp: \", check_stamp)\n",
    "avg_trans_e = 0\n",
    "avg_rot_e = 0\n",
    "cnt =0\n",
    "for stamp in df[0]:\n",
    "    if stamp > (first_stamp*1e-9 + 170):\n",
    "        print(\"STamp:\",stamp)\n",
    "        diffs = np.absolute(imgstamps - stamp)\n",
    "        image_index = diffs.argmin()\n",
    "        trans_e, rot_e = computeError(df, int(imgnames[image_index][:-4]), k, dist, 0.081, (9,9), T_cs)\n",
    "        if trans_e != -1 and rot_e != -1:\n",
    "            avg_trans_e = avg_trans_e + trans_e\n",
    "            avg_rot_e = avg_rot_e + rot_e\n",
    "            cnt = cnt +1\n",
    "print(\"Average translation error : \", avg_trans_e/cnt)\n",
    "print(\"Average rotational error : \", avg_rot_e/cnt)\n",
    "\n",
    "def iterate_timestamp_list(stamp_list, data_path, first_stamp, k, dist):\n",
    "    for last_stamp in stamp_list:\n",
    "        img_first = cv2.imread(os.path.join(data_path,str(first_stamp)+\".jpg\"), 0)\n",
    "        img_last = cv2.imread(os.path.join(data_path,str(last_stamp)+\".jpg\"), 0) #1650907892203072353 #1650907892153231019\n",
    "        #clahe = cv2.createCLAHE(clipLimit = 5)\n",
    "        #img_first = clahe.apply(img_first)\n",
    "        #img_last = clahe.apply(img_last)\n",
    "    \n",
    "        img_combined = np.hstack((img_first, img_last))\n",
    "        cv2.imshow(\"first and last images\", img_combined)\n",
    "        cv2.waitKey(0)\n",
    "    \n",
    "    \n",
    "        T_c0 = getPoseFromChessBoard(img_first, k, dist, 0.081)\n",
    "        print(\"First Pose WRT chessboard: \")\n",
    "        print(T_c0)\n",
    "    \n",
    "        print(\"----------------------------------------------------\") \n",
    "        computeError(df, first_stamp, k, dist, 0.081, (9,9), T_c0)\n",
    "        computeError(df, last_stamp, k, dist, 0.081, (9,9), T_c0)\n",
    "    \n",
    "        cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "247bd886",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "###########################################################################\n",
    "### NON-Overlapping Cams Calibration#######################################\n",
    "###########################################################################\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import gtsam\n",
    "import cv2\n",
    "\n",
    "\n",
    "def draw(img, corners, imgpts):\n",
    "    corners = corners.astype(np.int32)\n",
    "    imgpts = imgpts.astype(np.int32)\n",
    "    corner = tuple(corners[0].ravel())\n",
    "    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (0,0,255), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (255,0,0), 5)\n",
    "    return img\n",
    "\n",
    "\n",
    "def getPoseFromChessBoard(img_inp, k, dist, squarSize):\n",
    "    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "    objp = np.zeros((16*9,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:16,0:9].T.reshape(-1,2)\n",
    "    objp = objp * squarSize\n",
    "    axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)\n",
    "    axis = axis * squarSize\n",
    "    T_ci = np.zeros((4,4), np.float)\n",
    "    ## detect chesssboard\n",
    "    ret, corners = cv2.findChessboardCorners(img_inp, (16,9),None)\n",
    "    corners2 = []\n",
    "    if ret == True:\n",
    "        #print(\"corners found\")\n",
    "        corners2 = cv2.cornerSubPix(img_inp,corners,(11,11),(-1,-1),criteria)\n",
    "        # Find the rotation and translation vectors.\n",
    "        _,rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, k, dist)\n",
    "        rot_mat, _ = cv2.Rodrigues(rvecs)\n",
    "        ## get pose WRT chessboard T_ci\n",
    "        T_ic = np.vstack((np.hstack((rot_mat, tvecs)), np.array([0, 0, 0, 1])))\n",
    "        T_ci = np.linalg.inv(T_ic)\n",
    "        # project 3D points to image plane\n",
    "        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, k, dist)\n",
    "        cv2.drawChessboardCorners(img_inp, (16,9), corners2, ret)\n",
    "        img = cv2.cvtColor(img_inp,cv2.COLOR_GRAY2RGB)\n",
    "        img = draw(img,corners2,imgpts)\n",
    "        cv2.imshow('img',img)\n",
    "        cv2.waitKey(0) & 0xff\n",
    "    return T_ci, corners2\n",
    "    \n",
    "def get_extrinsic_estimate(image_path, tSTamp, df, k, dist,T_opti_initial, T_board_initial):\n",
    "    #Go through other camera images one by one.\n",
    "    # when calibration board is detected get pose T_board_side\n",
    "    #print(imgname)imgg\n",
    "    img_gray = cv2.imread(image_path, 0)\n",
    "    cv2.imshow(\"image\", img_gray)\n",
    "    cv2.waitKey(0)\n",
    "    T_board_side = getPoseFromChessBoard(img_gray, k, dist, 0.081)\n",
    "    if np.linalg.norm(T_board_side) == 0:\n",
    "        #print(\"Checkerboard not found\")\n",
    "        return\n",
    "    #Get the corresponding optitrack pose T_opti_rig. \n",
    "    #We want relative motion T_rig1_rig = T_rig1_opti * T_opti_rig\n",
    "    print(\"Image : \", tStamp) \n",
    "    ind_df = df[0].sub(tStamp).abs().idxmin()\n",
    "    print(\"index : \", ind_df)\n",
    "    quat = np.array(df.iloc[ind_df, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "    T_opti_rig = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "    T_rel_cur_1 = np.linalg.inv(T_opti_rig) @ T_opti_initial\n",
    "    #compute the extrinsics T_rig_side =  T_rig_rig1 * T_rig1_board* T_board_side\n",
    "    T_front_side = T_rel_cur_1 @ np.linalg.inv(T_board_initial) @ T_board_side\n",
    "    print(\"Extrinsic T_front_side: \", T_front_side)\n",
    "\n",
    "data_path_front=\"/data/05_16_2022_calib/bag3/cam0\"\n",
    "data_path_left=\"/data/05_16_2022_calib/bag3/cam5\"\n",
    "data_path_right=\"/data/05_16_2022_calib/bag3/cam6\"\n",
    "traj_path = \"/data/05_16_2022_calib/bag3/2022_05_16_18_34_56_gt.txt\"\n",
    "\n",
    "k_f = np.array([[874.039438699, 0, 364.550740159484],\n",
    "              [0, 874.787225463577, 286.6490604574],\n",
    "              [0, 0, 1]])\n",
    "dist_f = np.array([-0.24772900267450307, 0.1921058006879624, 2.2598774552204495e-05, 0.00309486514093092])\n",
    "\n",
    "k_s6 = np.array([[882.9391342187388, 0, 384.1878684084419],\n",
    "              [0, 882.8153341385736, 278.9079081016814],\n",
    "              [0, 0, 1]])\n",
    "dist_s6 = np.array([-0.21446184288970324, 0.14293621778218552, -0.0008679687005863199, 0.0013624375889589688])\n",
    "\n",
    "k_s5 = np.array([[877.1880881359045, 0, 367.273509202746],\n",
    "              [0, 877.57764402193, 285.9607389654299],\n",
    "              [0, 0, 1]])\n",
    "dist_s5 = np.array([-0.2228949253370626, 0.18736719172646038, 0.0010781783131958086, -0.0005436650475248478])\n",
    "\n",
    "first_stamp =  1652740514096921457 #side6 1652740503496921457\n",
    "\n",
    "#read the optitrack trajectory file\n",
    "df = pd.read_csv(traj_path, header=None,skipinitialspace=True, sep=' ')\n",
    "\n",
    "#get the pose of the camera WRT calib board . This is initial pose. T_board_rig1\n",
    "img_first = cv2.imread(os.path.join(data_path_front,str(first_stamp)+\".png\"), 0)\n",
    "T_board_front1, _ = getPoseFromChessBoard(img_first, k_f, dist_f, 0.081)\n",
    "print(T_board_front1)\n",
    "\n",
    "#get the corresponding optitrack pose. T_opti_rig1\n",
    "t =round(first_stamp*1e-9, 6)\n",
    "print(\"Image : \", first_stamp) \n",
    "ind_df = df[0].sub(t).abs().idxmin()\n",
    "print(\"index : \", ind_df)\n",
    "quat = np.array(df.iloc[ind_df, 4:8])\n",
    "rot = R.from_quat(quat).as_matrix()\n",
    "trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "T_opti_rig1 = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "\n",
    "imgnames = os.listdir(data_path_left)\n",
    "imgnames.sort()\n",
    "imgstamps = np.array([ float(img[:-4])*1e-9 for img in imgnames])\n",
    "\n",
    "#for imgname, tStamp in zip(imgnames[312 :412], imgstamps[312:412]):\n",
    "#    #get the pose of the camera WRT calib board . This is initial pose. T_board_rig1\n",
    "#    img_first = cv2.imread(os.path.join(data_path_front,imgname), 0)\n",
    "#    T_board_1 = getPoseFromChessBoard(img_first, k_f, dist_f, 0.081)\n",
    "#    \n",
    "#    #get the corresponding optitrack pose. T_opti_rig1\n",
    "#    print(\"Image : \", tStamp) \n",
    "#    ind_df = df[0].sub(tStamp).abs().idxmin()\n",
    "#    print(\"index : \", ind_df)\n",
    "#    quat = np.array(df.iloc[ind_df, 4:8])\n",
    "#    rot = R.from_quat(quat).as_matrix()\n",
    "#    trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "#    T_opti_1 = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "#    T_opti_board = T_opti_1 @ np.linalg.inv(T_board_1)\n",
    "#    print(\"T_opti_Board: data_path_right\",T_opti_board )\n",
    "#    #get_extrinsic_estimate(os.path.join(data_path_front,imgname),tStamp, df, k_f, dist_f, T_opti_rig1, T_board_front1)\n",
    "#   \n",
    "#for imgname, tStamp in zip(imgnames[:100], imgstamps[:100]):\n",
    "#    get_extrinsic_estimate(os.path.join(data_path_right,imgname),tStamp, df, k_s6, dist_s6, T_opti_rig1, T_board_front1)\n",
    "#\n",
    "#for imgname, tStamp in zip(imgnames[400:500], imgstamps[400:500]):\n",
    "#    get_extrinsic_estimate(os.path.join(data_path_left,imgname),tStamp, df, k_s5, dist_s5, T_opti_rig1, T_board_front1)\n",
    "\n",
    "#############################\n",
    "#### Optimization ###########\n",
    "#############################\n",
    "\n",
    "#We have initial estimates for landmarks(calibration target points)\n",
    "#rig poses frok optitrack and the relative poses between the rig and the individual cameras\n",
    "T_front_left = np.array([[-0.00728608, -0.02427516, -0.99967876 , 0.27547358],\n",
    " [ 0.02150753,  0.99947024, -0.02442685, -0.13939047],\n",
    " [ 0.99974214, -0.0216786,  -0.00676012, -0.03593664],\n",
    " [ 0. ,         0. ,         0.   ,       1.        ]])\n",
    "\n",
    "T_front_right = np.array( [[ 0.00935536,  0.01434981,  0.99985327 , 0.37059141],\n",
    " [-0.01839862,  0.99973023, -0.01417589, -0.13330094],\n",
    " [-0.99978696, -0.0182633,   0.00961685 , 0.00726508],\n",
    " [ 0.,          0.,          0.,          1.        ]])\n",
    "\n",
    "\n",
    "lms_board = np.zeros((16*9,3), np.float32)\n",
    "lms_board[:,:2] = np.mgrid[0:16,0:9].T.reshape(-1,2)\n",
    "lms_board = lms_board * 0.081\n",
    "lms_board = np.hstack((lms_board, np.ones((16*9, 1))))\n",
    "\n",
    "#print(lms_board)\n",
    "lms_1 = np.linalg.inv(T_board_front1) @ lms_board.T  \n",
    "\n",
    "# We build a factor graph with projectfactorPPP which optimizes over point, pose and pose\n",
    "# Read the front camera images. For first pose get T_first_board. This pose is the origin. We also have T_opti_rig1\n",
    "\n",
    "# for each of the other images, get the VO from optitrack T_rig1_opti * T_opti_rig = T_1_cur\n",
    "# insert the projection factor where the lms are WRT first cam = T_first_board * lms_board, \n",
    "# pose of the rig which coincides with first camera T_1_cur (this is indentity for the first frame)\n",
    "# pose of the component camera WRT first camera T_cur_left, T_cur_right\n",
    "from gtsam import symbol_shorthand\n",
    "L = symbol_shorthand.L\n",
    "X = symbol_shorthand.X\n",
    "B = symbol_shorthand.B\n",
    "import gtsam_unstable\n",
    "from gtsam_unstable import ProjectionFactorPPPCal3_S2\n",
    "from gtsam import (Cal3_S2, LevenbergMarquardtOptimizer,\n",
    "                         GenericProjectionFactorCal3_S2, Marginals,\n",
    "                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Point2,\n",
    "                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)\n",
    "\n",
    "\n",
    "gtsam_Kf = Cal3_S2(k_f[0,0], k_f[1,1], 0.0, k_f[0,2], k_f[1,2])\n",
    "gtsam_Kleft = Cal3_S2(k_s5[0,0], k_s5[1,1], 0.0, k_s5[0,2], k_s5[1,2])\n",
    "gtsam_Kright = Cal3_S2(k_s6[0,0], k_s6[1,1], 0.0, k_s6[0,2], k_s6[1,2])\n",
    "\n",
    "\n",
    "# Define the camera observation noise model\n",
    "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 2.0)  # one pixel in u and v\n",
    "#0.3 rad and 0.1 m\n",
    "pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.05, 0.05, 0.05]))\n",
    "# 0.1 m\n",
    "point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)\n",
    "\n",
    "#Initialze graph and initial values\n",
    "graph = NonlinearFactorGraph()\n",
    "initial_estimate = Values()\n",
    "\n",
    "img_list = list(zip(imgnames, imgstamps))\n",
    "img_index = 0\n",
    "\n",
    "# For each image\n",
    "for imgname, tStamp in img_list[0:600:10]:\n",
    "    print(imgname, tStamp)\n",
    "    img_index = img_index + 1\n",
    "    # read the image\n",
    "    img_first = cv2.imread(os.path.join(data_path_front,imgname), 0)\n",
    "    #undistort the image     \n",
    "    imgg = cv2.undistort(img_first, k_f, dist_f, None, k_f)\n",
    "    # get the pose of the image WRT board and the corresponding image points\n",
    "    T_board_front, imgPoints = getPoseFromChessBoard(imgg, k_f, np.zeros((1,4)), 0.081)\n",
    "    cam_ind = 0\n",
    "    # if no board detected in the fornt camera go to left ca,\n",
    "    if np.linalg.norm(T_board_front) == 0:\n",
    "        img_left = cv2.imread(os.path.join(data_path_left,imgname), 0)\n",
    "        imgg = cv2.undistort(img_left,k_s5, dist_s5, None, k_s5)\n",
    "        T_board_left, imgPoints = getPoseFromChessBoard(imgg, k_s5, np.zeros((1,4)), 0.081)\n",
    "        # if no board detected in left camera go to right\n",
    "        if np.linalg.norm(T_board_left) == 0:\n",
    "            img_right = cv2.imread(os.path.join(data_path_right,imgname), 0)\n",
    "            imgg = cv2.undistort(img_right,k_s6, dist_s6, None, k_s6)\n",
    "            T_board_right, imgPoints = getPoseFromChessBoard(imgg, k_s6, np.zeros((1,4)), 0.081)\n",
    "            if np.linalg.norm(T_board_right) == 0:\n",
    "                print(\"Checkerboard not found in any of the images\")\n",
    "                cam_ind = -1\n",
    "                continue\n",
    "            else:\n",
    "                cam_ind = 2\n",
    "        else:\n",
    "            cam_ind = 1\n",
    "    if len(imgPoints) != 0:\n",
    "        cv2.drawChessboardCorners(imgg, (16,9), imgPoints, True)\n",
    "        cv2.imshow('img',imgg)\n",
    "        cv2.waitKey(0) & 0xff\n",
    "    else:\n",
    "        continue\n",
    "    \n",
    "        \n",
    "    #get the corresponding optitrack pose. T_opti_rig1\n",
    "    print(\"Image : \", tStamp) \n",
    "    ind_df = df[0].sub(tStamp).abs().idxmin()\n",
    "    print(\"index : \", ind_df)\n",
    "    quat = np.array(df.iloc[ind_df, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "    T_opti_rig = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "    # get the relative pose of the current frame WRT the refrence frame . \n",
    "    T_1_cur = np.linalg.inv(T_opti_rig1) @ T_opti_rig\n",
    "    #  depending on the camera it is seen in get the 3d-2d correspondences\n",
    "    #For each landmark add a prior if needed\n",
    "    assert (len(imgPoints) == lms_1.shape[1])\n",
    "    \n",
    "    #Insert the pose of the rig/front camera WRT the starting position\n",
    "    initial_estimate.insert(X(img_index), Pose3(T_1_cur))\n",
    "    #factor = PriorFactorPose3(X(img_index), Pose3(T_1_cur), pose_noise)\n",
    "    #graph.push_back(factor)\n",
    "    \n",
    "    K = gtsam_Kf\n",
    "    if cam_ind == 1 :\n",
    "        K = gtsam_Kleft\n",
    "    elif cam_ind == 2:\n",
    "        K = gtsam_Kright\n",
    "    # Add the projectionPPP factor for each landmark with the measured corners\n",
    "    for i in range(lms_1.shape[1]):\n",
    "        lm = Point3(lms_1[0:3, i])\n",
    "        #factor = PriorFactorPoint3(L(i), lm, point_noise)\n",
    "        #graph.push_back(factor)\n",
    "        meas = Point2(imgPoints[i].ravel())\n",
    "        factor = ProjectionFactorPPPCal3_S2(meas, measurement_noise, X(img_index),B(cam_ind), L(i), K )\n",
    "        graph.push_back(factor)\n",
    "\n",
    "# add prior on the first pose and the front camera ad identity. This our refernce frame. We assume\n",
    "# here that the front camera frame is coinciding with the first image frame\n",
    "pose_noise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.001, 0.001, 0.001]))\n",
    "factor = PriorFactorPose3(B(0),Pose3(np.eye(4)), pose_noise2)\n",
    "graph.push_back(factor)\n",
    "initiPose = Pose3(np.eye(4))\n",
    "factor = PriorFactorPose3(X(354), initiPose, pose_noise)\n",
    "graph.push_back(factor)\n",
    "#factor = PriorFactorPoint3(L(0), Point3(lms_1[0:3, 0]), point_noise)\n",
    "#graph.push_back(factor)\n",
    "\n",
    "############################\n",
    "## Initial Estimates########\n",
    "############################\n",
    "initial_estimate.insert(X(354), Pose3(np.eye(4)))\n",
    "initial_estimate.insert(B(0), Pose3(np.eye(4)))\n",
    "initial_estimate.insert(B(1), Pose3(T_front_left))\n",
    "initial_estimate.insert(B(2), Pose3(T_front_right))\n",
    "for i in range(lms_1.shape[1]):\n",
    "    lm = Point3(lms_1[0:3, i])\n",
    "    initial_estimate.insert(L(i), lm)\n",
    "\n",
    "\n",
    "params = gtsam.LevenbergMarquardtParams()\n",
    "params.setDiagonalDamping(True)\n",
    "params.setVerbosityLM(\"SUMMARY\")\n",
    "optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)\n",
    "result = optimizer.optimize()\n",
    "result.print('Final results:\\n')\n",
    "print('initial error = {}'.format(graph.error(initial_estimate)))\n",
    "print('final error = {}'.format(graph.error(result)))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6f998a8e",
   "metadata": {},
   "outputs": [],
   "source": [
    "###########################################################################\n",
    "### NON-Overlapping Cams Calibration#######################################\n",
    "###########################################################################\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import os\n",
    "from datetime import datetime\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import patches\n",
    "from scipy.spatial.transform import Rotation as R\n",
    "import gtsam\n",
    "import cv2\n",
    "\n",
    "\n",
    "def draw(img, corners, imgpts):\n",
    "    corners = corners.astype(np.int32)\n",
    "    imgpts = imgpts.astype(np.int32)\n",
    "    corner = tuple(corners[0].ravel())\n",
    "    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (0,0,255), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0,255,0), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (255,0,0), 5)\n",
    "    return img\n",
    "\n",
    "\n",
    "def getPoseFromChessBoard(img_inp, k, dist, squarSize):\n",
    "    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "    objp = np.zeros((16*9,3), np.float32)\n",
    "    objp[:,:2] = np.mgrid[0:16,0:9].T.reshape(-1,2)\n",
    "    objp = objp * squarSize\n",
    "    axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)\n",
    "    axis = axis * squarSize\n",
    "    T_ci = np.zeros((4,4), np.float)\n",
    "    ## detect chesssboard\n",
    "    ret, corners = cv2.findChessboardCorners(img_inp, (16,9),None)\n",
    "    corners2 = []\n",
    "    if ret == True:\n",
    "        #print(\"corners found\")\n",
    "        corners2 = cv2.cornerSubPix(img_inp,corners,(11,11),(-1,-1),criteria)\n",
    "        # Find the rotation and translation vectors.\n",
    "        _,rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, k, dist)\n",
    "        rot_mat, _ = cv2.Rodrigues(rvecs)\n",
    "        ## get pose WRT chessboard T_ci\n",
    "        T_ic = np.vstack((np.hstack((rot_mat, tvecs)), np.array([0, 0, 0, 1])))\n",
    "        T_ci = np.linalg.inv(T_ic)\n",
    "        # project 3D points to image plane\n",
    "        imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, k, dist)\n",
    "        cv2.drawChessboardCorners(img_inp, (16,9), corners2, ret)\n",
    "        img = cv2.cvtColor(img_inp,cv2.COLOR_GRAY2RGB)\n",
    "        img = draw(img,corners2,imgpts)\n",
    "        cv2.imshow('img',img)\n",
    "        cv2.waitKey(0) & 0xff\n",
    "    return T_ci, corners2\n",
    "##########################################################################\n",
    "######## These are paths and camera parameters I used, just for an example\n",
    "#################################################################################\n",
    "data_path_front=\"/data/05_16_2022_calib/bag3/cam0\"\n",
    "data_path_left=\"/data/05_16_2022_calib/bag3/cam5\"\n",
    "data_path_right=\"/data/05_16_2022_calib/bag3/cam6\"\n",
    "traj_path = \"/data/05_16_2022_calib/bag3/2022_05_16_18_34_56_gt.txt\"\n",
    "\n",
    "k_f = np.array([[874.039438699, 0, 364.550740159484],\n",
    "              [0, 874.787225463577, 286.6490604574],\n",
    "              [0, 0, 1]])\n",
    "dist_f = np.array([-0.24772900267450307, 0.1921058006879624, 2.2598774552204495e-05, 0.00309486514093092])\n",
    "\n",
    "k_s6 = np.array([[882.9391342187388, 0, 384.1878684084419],\n",
    "              [0, 882.8153341385736, 278.9079081016814],\n",
    "              [0, 0, 1]])\n",
    "dist_s6 = np.array([-0.21446184288970324, 0.14293621778218552, -0.0008679687005863199, 0.0013624375889589688])\n",
    "\n",
    "k_s5 = np.array([[877.1880881359045, 0, 367.273509202746],\n",
    "              [0, 877.57764402193, 285.9607389654299],\n",
    "              [0, 0, 1]])\n",
    "dist_s5 = np.array([-0.2228949253370626, 0.18736719172646038, 0.0010781783131958086, -0.0005436650475248478])\n",
    "\n",
    "first_stamp =  1652740514096921457 \n",
    "\n",
    "############################################################################\n",
    "\n",
    "\n",
    "#read the optitrack trajectory file into a pandas dataframe\n",
    "# the trajectory file is in TUM format and contains poses of the multi-camera rig\n",
    "# format -> timestamp tx ty tz qx qy qz qw\n",
    "df = pd.read_csv(traj_path, header=None,skipinitialspace=True, sep=' ')\n",
    "\n",
    "#Our multi-camera rig has 3 non-overlapping cameras front, left and right.\n",
    "#front camera is considered as the reference coordinate frame for the rig.\n",
    "#Read the first image in front camera in the sequence. \n",
    "#get the pose of the front camera WRT calib board for the first frame in the sequence. \n",
    "#his pose is the origin, initial pose of the rig.\n",
    "img_first = cv2.imread(os.path.join(data_path_front,str(first_stamp)+\".png\"), 0)\n",
    "T_board_front1, _ = getPoseFromChessBoard(img_first, k_f, dist_f, 0.081)\n",
    "print(T_board_front1)\n",
    "\n",
    "#get the corresponding optitrack pose. T_opti_front1\n",
    "t =round(first_stamp*1e-9, 6)\n",
    "print(\"Image : \", first_stamp) \n",
    "ind_df = df[0].sub(t).abs().idxmin()\n",
    "print(\"index : \", ind_df)\n",
    "quat = np.array(df.iloc[ind_df, 4:8])\n",
    "rot = R.from_quat(quat).as_matrix()\n",
    "trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "T_opti_front1 = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "\n",
    "imgnames = os.listdir(data_path_left)\n",
    "imgnames.sort()\n",
    "imgstamps = np.array([ float(img[:-4])*1e-9 for img in imgnames])\n",
    "\n",
    "############################################\n",
    "######## Pose graph Optimization ###########\n",
    "############################################\n",
    "\n",
    "#We have initial estimates for landmarks(calibration target points)\n",
    "#rig poses from optitrack and the relative poses between the rig and the individual cameras\n",
    "# Front camera is assumed to be the reference frame for the multi-camera rig.\n",
    "\n",
    "T_front_left = np.array([[-0.00728608, -0.02427516, -0.99967876 , 0.27547358],\n",
    " [ 0.02150753,  0.99947024, -0.02442685, -0.13939047],\n",
    " [ 0.99974214, -0.0216786,  -0.00676012, -0.03593664],\n",
    " [ 0. ,         0. ,         0.   ,       1.        ]])\n",
    "\n",
    "T_front_right = np.array( [[ 0.00935536,  0.01434981,  0.99985327 , 0.37059141],\n",
    " [-0.01839862,  0.99973023, -0.01417589, -0.13330094],\n",
    " [-0.99978696, -0.0182633,   0.00961685 , 0.00726508],\n",
    " [ 0.,          0.,          0.,          1.        ]])\n",
    "\n",
    "### landmarks are the chessboard corners\n",
    "lms_board = np.zeros((16*9,3), np.float32)\n",
    "lms_board[:,:2] = np.mgrid[0:16,0:9].T.reshape(-1,2)\n",
    "lms_board = lms_board * 0.081\n",
    "lms_board = np.hstack((lms_board, np.ones((16*9, 1))))\n",
    "\n",
    "#Convert the landmarks from the board coordinate frame to \n",
    "# multi-camera rig coordinate frame (front camera coordinate frame)\n",
    "lms_1 = np.linalg.inv(T_board_front1) @ lms_board.T  \n",
    "\n",
    "# We build a factor graph with projectfactorPPP which optimizes over point, pose (rig) and pose (component camera)\n",
    "# insert the projection factor where the lms are WRT first cam = T_first_board * lms_board, \n",
    "# pose of the rig which coincides with first camera T_1_cur (this is indentity for the first frame)\n",
    "# pose of the component camera WRT first camera T_cur_left, T_cur_right\n",
    "from gtsam import symbol_shorthand\n",
    "L = symbol_shorthand.L\n",
    "X = symbol_shorthand.X\n",
    "B = symbol_shorthand.B\n",
    "import gtsam_unstable\n",
    "from gtsam_unstable import ProjectionFactorPPPCal3_S2\n",
    "from gtsam import (Cal3_S2, LevenbergMarquardtOptimizer,\n",
    "                         GenericProjectionFactorCal3_S2, Marginals,\n",
    "                         NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Point2,\n",
    "                         Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, Values)\n",
    "\n",
    "\n",
    "gtsam_Kf = Cal3_S2(k_f[0,0], k_f[1,1], 0.0, k_f[0,2], k_f[1,2])\n",
    "gtsam_Kleft = Cal3_S2(k_s5[0,0], k_s5[1,1], 0.0, k_s5[0,2], k_s5[1,2])\n",
    "gtsam_Kright = Cal3_S2(k_s6[0,0], k_s6[1,1], 0.0, k_s6[0,2], k_s6[1,2])\n",
    "\n",
    "\n",
    "# Define the camera observation noise model\n",
    "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 2.0)  # one pixel in u and v\n",
    "#0.3 rad and 0.1 m\n",
    "pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.05, 0.05, 0.05]))\n",
    "# 0.1 m\n",
    "point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)\n",
    "\n",
    "#Initialze graph and initial values\n",
    "graph = NonlinearFactorGraph()\n",
    "initial_estimate = Values()\n",
    "\n",
    "img_list = list(zip(imgnames, imgstamps))\n",
    "img_index = 0\n",
    "\n",
    "# For each image\n",
    "for imgname, tStamp in img_list[0:600:10]:\n",
    "    print(imgname, tStamp)\n",
    "    img_index = img_index + 1\n",
    "    # read the image in the front camera\n",
    "    img_first = cv2.imread(os.path.join(data_path_front,imgname), 0)\n",
    "    #undistort the image     \n",
    "    imgg = cv2.undistort(img_first, k_f, dist_f, None, k_f)\n",
    "    # get the pose of the image WRT board and the corresponding image points\n",
    "    T_board_front, imgPoints = getPoseFromChessBoard(imgg, k_f, np.zeros((1,4)), 0.081)\n",
    "    cam_ind = 0\n",
    "    # if no board detected in the fornt camera go to left camera\n",
    "    if np.linalg.norm(T_board_front) == 0:\n",
    "        img_left = cv2.imread(os.path.join(data_path_left,imgname), 0)\n",
    "        imgg = cv2.undistort(img_left,k_s5, dist_s5, None, k_s5)\n",
    "        T_board_left, imgPoints = getPoseFromChessBoard(imgg, k_s5, np.zeros((1,4)), 0.081)\n",
    "        # if no board detected in left camera go to right\n",
    "        if np.linalg.norm(T_board_left) == 0:\n",
    "            img_right = cv2.imread(os.path.join(data_path_right,imgname), 0)\n",
    "            imgg = cv2.undistort(img_right,k_s6, dist_s6, None, k_s6)\n",
    "            T_board_right, imgPoints = getPoseFromChessBoard(imgg, k_s6, np.zeros((1,4)), 0.081)\n",
    "            if np.linalg.norm(T_board_right) == 0:\n",
    "                print(\"Checkerboard not found in any of the images\")\n",
    "                cam_ind = -1\n",
    "                continue\n",
    "            else:\n",
    "                cam_ind = 2\n",
    "        else:\n",
    "            cam_ind = 1\n",
    "    # if no calibration target is detected in any of the cameras go to next\n",
    "    if len(imgPoints) != 0:\n",
    "        cv2.drawChessboardCorners(imgg, (16,9), imgPoints, True)\n",
    "        cv2.imshow('img',imgg)\n",
    "        cv2.waitKey(0) & 0xff\n",
    "    else:\n",
    "        continue\n",
    "    \n",
    "        \n",
    "    #get the corresponding optitrack pose. T_opti_rig1\n",
    "    print(\"Image : \", tStamp) \n",
    "    ind_df = df[0].sub(tStamp).abs().idxmin()\n",
    "    print(\"index : \", ind_df)\n",
    "    quat = np.array(df.iloc[ind_df, 4:8])\n",
    "    rot = R.from_quat(quat).as_matrix()\n",
    "    trans = np.array(df.iloc[ind_df,1:4]).reshape(3,1)\n",
    "    T_opti_cur = np.vstack((np.hstack((rot, trans)), np.array([0,0,0,1])))\n",
    "    \n",
    "    # get the relative pose of the current frame WRT the initial frame(origin) . \n",
    "    T_1_cur = np.linalg.inv(T_opti_front1) @ T_opti_cur\n",
    "    #  depending on the camera it is seen in get the 3d-2d correspondences\n",
    "    #For each landmark add a prior if needed\n",
    "    assert (len(imgPoints) == lms_1.shape[1])\n",
    "    \n",
    "    #Insert the pose of the rig/front camera WRT the starting position\n",
    "    initial_estimate.insert(X(img_index), Pose3(T_1_cur))\n",
    "    #factor = PriorFactorPose3(X(img_index), Pose3(T_1_cur), pose_noise)\n",
    "    #graph.push_back(factor)\n",
    "    \n",
    "    K = gtsam_Kf\n",
    "    if cam_ind == 1 :\n",
    "        K = gtsam_Kleft\n",
    "    elif cam_ind == 2:\n",
    "        K = gtsam_Kright\n",
    "    # Add the projectionPPP factor for each landmark with the measured corners\n",
    "    for i in range(lms_1.shape[1]):\n",
    "        lm = Point3(lms_1[0:3, i])\n",
    "        #factor = PriorFactorPoint3(L(i), lm, point_noise)\n",
    "        #graph.push_back(factor)\n",
    "        meas = Point2(imgPoints[i].ravel())\n",
    "        factor = ProjectionFactorPPPCal3_S2(meas, measurement_noise, X(img_index),B(cam_ind), L(i), K )\n",
    "        graph.push_back(factor)\n",
    "\n",
    "# add prior on the first pose and the front camera as identity. This our refernce frame. \n",
    "pose_noise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.001, 0.001, 0.001]))\n",
    "factor = PriorFactorPose3(B(0),Pose3(np.eye(4)), pose_noise2)\n",
    "graph.push_back(factor)\n",
    "initiPose = Pose3(np.eye(4))\n",
    "factor = PriorFactorPose3(X(354), initiPose, pose_noise)\n",
    "graph.push_back(factor)\n",
    "#factor = PriorFactorPoint3(L(0), Point3(lms_1[0:3, 0]), point_noise)\n",
    "#graph.push_back(factor)\n",
    "\n",
    "############################\n",
    "## Initial Estimates########\n",
    "############################\n",
    "initial_estimate.insert(X(354), Pose3(np.eye(4)))\n",
    "initial_estimate.insert(B(0), Pose3(np.eye(4)))\n",
    "initial_estimate.insert(B(1), Pose3(T_front_left))\n",
    "initial_estimate.insert(B(2), Pose3(T_front_right))\n",
    "for i in range(lms_1.shape[1]):\n",
    "    lm = Point3(lms_1[0:3, i])\n",
    "    initial_estimate.insert(L(i), lm)\n",
    "\n",
    "\n",
    "params = gtsam.LevenbergMarquardtParams()\n",
    "params.setDiagonalDamping(True)\n",
    "params.setVerbosityLM(\"SUMMARY\")\n",
    "optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)\n",
    "result = optimizer.optimize()\n",
    "result.print('Final results:\\n')\n",
    "print('initial error = {}'.format(graph.error(initial_estimate)))\n",
    "print('final error = {}'.format(graph.error(result)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f3f52246",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.inv(T_front_right)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d440b571",
   "metadata": {},
   "outputs": [],
   "source": [
    "###########################################################################\n",
    "### PROCESS Groundtruth Data -GPS ############################################\n",
    "############################################################################\n",
    "import glob\n",
    "import pickle\n",
    "import utm\n",
    "2 - 0.460\n",
    "3 - 0.41\n",
    "4 - 0.393\n",
    "5 - 0.361\n",
    "#read the pkl file of gps\n",
    "data_path = \"/data/05_05_2022_whoi/fixed_expo\"\n",
    "files = glob.glob(os.path.join(data_path, \"*.pkl\")) #os.listdir(os.path.join(data_path)\n",
    "files.sort()\n",
    "print(files[0])\n",
    "all_data=[]\n",
    "for f in files:\n",
    "    df = pd.read_pickle(f)\n",
    "    df_gps = df['robot']['gps']\n",
    "    df_gps.reset_index(inplace=True)\n",
    "    print(\"Read \", f)\n",
    "    for i in range(df_gps.shape[0]):\n",
    "        x, y, _,_ = utm.from_latlon(df_gps.iloc[i,1], df_gps.iloc[i,2])\n",
    "        z = df_gps.iloc[i,3]\n",
    "        t = pd.Timestamp(df_gps.iloc[i,0]).timestamp()\n",
    "        all_data.append([t, -1.0*y, z, x, 0.0, 0.0, 0.0, 1.0])\n",
    "        #print(t,\" \",x,\" \",y,\" \",z)\n",
    "np_data = np.array(all_data, np.float64)\n",
    "pd_data = pd.DataFrame(np_data)\n",
    "print(pd_data.head())\n",
    "print(pd_data.tail())\n",
    "pd_data[[1,2,3]] = pd_data[[1,2,3]] - pd_data.iloc[0, 1:4]\n",
    "print(pd_data.head())\n",
    "pd_data.to_csv(os.path.join(data_path, \"gps_groundtruth.txt\"), sep=' ', header=False, index=False)\n",
    "print(pd_data.tail())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6a80860a",
   "metadata": {},
   "outputs": [],
   "source": [
    "./LFSlamapp --config_file /data/05_15_2022/bag2/lf_slam_config.cfg --log_file /data/05_15_2022/bag2/results/pose_stats_05_15_2022_5cams_1.txt --traj_file /data/05_15_2022/bag2/results/traj_05_15_2022_5cams_1.txt -v 1\n",
    "./LFSlamapp --config_file /data/05_15_2022/bag2/lf_slam_config.cfg --log_file /data/05_15_2022/bag2/results/pose_stats_05_15_2022_5cams_2.txt --traj_file /data/05_15_2022/bag2/results/traj_05_15_2022_5cams_2.txt -v 1\n",
    "./LFSlamapp --config_file /data/05_15_2022/bag2/lf_slam_config.cfg --log_file /data/05_15_2022/bag2/results/pose_stats_05_15_2022_5cams_3.txt --traj_file /data/05_15_2022/bag2/results/traj_05_15_2022_5cams_3.txt -v 1"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
